#include <MPython.h>
#include <math.h>

#if CONFIG_FREERTOS_UNICORE
#define ARDUINO_RUNNING_CORE 0
#else
#define ARDUINO_RUNNING_CORE 1
#endif

DFRobot_NeoPixel pixels;
DFRobot_SSD1306_I2C display = DFRobot_SSD1306_I2C();


MPython::MPython(){}

void MPython::begin(){
    pixels.begin(17, 3, 100);
    rgb.write(-1,0);
    display.begin(0x3c);
    buzz.off();
    accelerometer.init();
}

void MPython::setTouchThreshold(uint32_t threshold)
{
    threshold = constrain(threshold, 0, 80);
    touchPadP.threshold = threshold;
    touchPadY.threshold = threshold;
    touchPadT.threshold = threshold;
    touchPadH.threshold = threshold;
    touchPadO.threshold = threshold;
    touchPadN.threshold = threshold;
}

Button::Button(uint8_t _io)
    :pressedCb(NULL),unpressedCb(NULL),io(_io),io1(-1),io2(-1),pinNum(1),prevState(false)
{
    pinMode(io,INPUT|PULLUP);
}

Button::Button(uint8_t _io1, uint8_t _io2)
    :pressedCb(NULL),unpressedCb(NULL),io(-1),io1(_io1),io2(_io2),pinNum(2)
{
    pinMode(io1,INPUT|PULLUP);
    pinMode(io2,INPUT|PULLUP);
}

bool Button::isPressed(void)
{
    int retry = 10;
    if(pinNum == 1){
        if(digitalRead(io) == 0){
            while(retry--){
                if(digitalRead(io))
                    return false;
                delay(5);
            }
            if((digitalRead(io)==0) && (digitalRead(io==0?2:0) != 0))
                return true;
        }
        return false;
    }else{
        if((digitalRead(io1) == 0) && (digitalRead(io2) == 0)){
            while(retry--){
                if(digitalRead(io1) || digitalRead(io2))
                    return false;
                delay(5);
            }
            if((digitalRead(io1)==0) && (digitalRead(io2) == 0))
                return true;
        }
        return false;
    }
}

void Button::setPressedCallback(CBFunc cb)
{
    static char name[]={"buttonX"};
    if(pinNum == 1)
        name[6] = io+'0';
    else
        name[6] = io1 + io2 +'0';
    if(pressedCb == NULL && unpressedCb == NULL){
        xTaskCreatePinnedToCore(Button::taskLoop, name, 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
    }
    pressedCb = cb;
}

void Button::setUnPressedCallback(CBFunc cb)
{
    static char name[]={"buttonX"};
    if(pinNum == 1)
        name[6] = io+'0';
    else
        name[6] = io1 + io2 +'0';
    if(unpressedCb == NULL && pressedCb == NULL){
        xTaskCreatePinnedToCore(Button::taskLoop, name, 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
    }
    unpressedCb = cb;
}


RGB::RGB()
{
    c[0]=0,c[1]=0,c[2]=0;
    _brightness = -1;
}

void RGB::write(int8_t index, uint8_t r, uint8_t g, uint8_t b)
{
    if(index>=0 && index<=2){
        pixels.setRangeColor(index, index, pixels.rgbToColor(round(r), round(g), round(b)));
    }else{
        pixels.setRangeColor(0, 2, pixels.rgbToColor(round(r), round(g), round(b)));
    }
    delay(5);
}

void RGB::brightness(uint8_t b)
{
    if(_brightness == b) return;
    _brightness = max(min(b,9),0);
    pixels.setBrightness((uint8_t)map(_brightness, 0, 9, 0, 255));
}

uint8_t RGB::brightness()
{
    return _brightness;
}

Buzz::Buzz()
    :_freq(0),_on(false)
{
    ledcSetup(0, 500, 10);
    ledcWrite(0, 512);
}

void Buzz::on(void)
{
    ledcAttachPin(16, 0);
    _on = true;
}

void Buzz::off(void)
{
    ledcDetachPin(16);
    /** debug by LX.防止电容反向充电导致蜂鸣器发声延时
      */
    digitalWrite(16, 1);
    _on = false;
}

bool Buzz::isOn(void)
{
    return _on;
}

void Buzz::freq(uint32_t _freq)
{
    if(!_on){
        on();
        _on = true;
    }
    ledcWrite(0, 512);
    ledcWriteTone(0,_freq);
}

void Buzz::freq(uint32_t _freq, Beat beat)
{
    long delay_time = 0;
    switch (beat) {
        case BEAT_1: 
            delay_time = 1000; break;
        case BEAT_1_2:
            delay_time = 500; break;
        case BEAT_1_4:
            delay_time = 250; break;
        case BEAT_2:
            delay_time = 2000; break;
        case BEAT_4:
            delay_time = 4000; break;
    }
    if(!_on){
        on();
        _on = true;
    }
    ledcWriteTone(0,_freq);
    if(delay_time)
        delay(delay_time);
    off();
}

void Buzz::freq(uint32_t _freq, double beat)
{
    if(beat>0.125 && beat<=0.25)
        freq(_freq, BEAT_1_4);
    else if(beat>0.25 && beat<=0.5)
        freq(_freq, BEAT_1_2);
    else if(beat>0.5 && beat<=1)
        freq(_freq, BEAT_1);
    else if(beat>1 && beat<=2)
        freq(_freq, BEAT_2);
    else if(beat>2 && beat<=4)
        freq(_freq, BEAT_4);
    else
        freq(_freq, BEAT_1);
}


TouchPad::TouchPad(uint8_t _io)
    :touchedCb(NULL),untouchedCb(NULL),io(_io),valuePrevPrev(80),valuePrev(80),value(80), threshold(50)
{
    
}

uint32_t TouchPad::read(void)
{
    if((touchedCb == NULL) && (untouchedCb == NULL)){
        return touchRead(io);
    }
    return value;
}

bool TouchPad::isTouched(void)
{
    valuePrevPrev = valuePrev;
    valuePrev = value;
    value = touchRead(io);
    //if(value < threshold )
    //    Serial.printf("--touch io=%d----valuePrevPrev=%d--valuePrev=%d---value=%d---\n",io,valuePrevPrev,valuePrev,value);
    return ((value<threshold) && (valuePrev<threshold) && (valuePrevPrev<threshold));
}

void TouchPad::setTouchedCallback(CBFunc cb)
{
    static char name[]={"touchX"};
    name[5] = io+'0';
    if((touchedCb == NULL) && (untouchedCb == NULL)){
        xTaskCreatePinnedToCore(TouchPad::taskLoop, name, 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
    }
    touchedCb = cb;
}

void TouchPad::setUnTouchedCallback(CBFunc cb)
{
    static char name[]={"touchX"};
    name[5] = io+'0';
    if((touchedCb == NULL) && (untouchedCb == NULL)){
        xTaskCreatePinnedToCore(TouchPad::taskLoop, name, 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
    }
    untouchedCb = cb;
}

AnalogPin::AnalogPin(uint8_t _io)
    :io(_io)
{
}

uint16_t AnalogPin::read()
{
    return analogRead(io);
}


static uint8_t i2cread(void)
{
    return Wire.read();
}

static void i2cwrite(uint8_t x)
{
    Wire.write((uint8_t)x);
}

static void writeRegister(uint8_t i2cAddress, uint8_t reg, uint8_t value)
{
    Wire.setClock(400000);
    Wire.beginTransmission(i2cAddress);
    i2cwrite(reg);
    i2cwrite(value);
    Wire.endTransmission();
}
static int16_t readRegister(uint8_t i2cAddress, uint8_t reg)
{
    Wire.setClock(400000);
    Wire.beginTransmission(i2cAddress);
    i2cwrite((uint8_t)reg);
    Wire.endTransmission();
    Wire.requestFrom(i2cAddress, (uint8_t)2);
    return (int16_t)(i2cread() | (i2cread()<<8));
}

MSA300::MSA300()
{
    for(uint8_t i=0; i<7; i++){
        GestureHandle[i] = NULL;
    }
}

void MSA300::init(void)
{
    Wire.begin();
    writeRegister(MSA300_I2C_ADDR, 0x0F, 0x08);
    writeRegister(MSA300_I2C_ADDR, 0x11, 0x00);
    delay(100);
}

float MSA300::getRawX(void)
{
    uint16_t v=0,retry=3;
    do{
        v = readRegister(MSA300_I2C_ADDR, 0x02);
        if(v == 0xffff){
            init();
        }
    }while((v==0xffff) && (--retry));
    rawX = ((int16_t)v)*1.0/4/4096;
    return rawX;
}

float MSA300::getRawY(void)
{
    uint16_t v=0,retry=3;
    do{
        v = readRegister(MSA300_I2C_ADDR, 0x04);
        if(v == 0xffff){
            init();
        }
    }while((v == 0xffff) && (--retry));
    rawY = ((int16_t)v)*1.0/4/4096;
    return rawY;
}

float MSA300::getRawZ(void)
{
    uint16_t v=0,retry=3;
    do{
        v = readRegister(MSA300_I2C_ADDR, 0x06);
        if(v == 0xffff){
            init();
        }
    }while((v == 0xffff) && (--retry));
    rawZ = ((int16_t)v)*1.0/4/4096;
    return rawZ;
}


float MSA300::getX(void)
{
    if(!isGestureEnable){
        return getRawX() * 1000; // g to mg
    }
    return rawX * 1000;
}

float MSA300::getY(void)
{
    if(!isGestureEnable){
        return getRawY() * 1000; // g to mg
    }
    return rawY * 1000;
}

float MSA300::getZ(void)
{
    if(!isGestureEnable){
        return getRawZ() * 1000; // g to mg
    }
    return rawZ * 1000;
}

float MSA300::getStrength(void)
{
    float x=getX();
    float y=getY();
    float z=getZ();
    return sqrt(x*x+y*y+z*z);
}

void MSA300::onGesture(Gesture gesture, mpythonGestureHandlePtr body)
{
    if(!isGestureEnable){
        xTaskCreatePinnedToCore(MSA300::taskLoop, "gestureTask", 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
        isGestureEnable = true;
    }
    GestureHandle[gesture] = body;
}

bool MSA300::isGesture(Gesture gesture)
{
    if(!isGestureEnable){
        xTaskCreatePinnedToCore(MSA300::taskLoop, "gestureTask", 2048, this, 1, NULL, ARDUINO_RUNNING_CORE);
        isGestureEnable = true;
    }
    if(gesture != currentGesture) 
        return false;
    return true;
}

MPython mPython;
Button buttonA(0),buttonB(2),buttonAB(0,2);
RGB rgb;
Buzz buzz;
TouchPad touchPadP(27),touchPadY(14),touchPadT(12),touchPadH(13),touchPadO(15),touchPadN(4);
AnalogPin light(39),sound(A0);
MSA300 accelerometer;